package touchphysics.physics.collision;

import touchphysics.math.Vector2f;

/**
 
 * @author Valerij Wittenbeck
 *
 */
public class EdgeSweep {

	/**  */
	class ProjectedVertex {
		/**  */
		public int vertex;
		/**  */
		public boolean isA;
		/**  */
		public float distance;
		
		/**  */
		public ProjectedVertex next;
		/**  */
		public ProjectedVertex previous;
	
		/** 
		 * 
		 * @param vertex 
		 * @param isA 
		 * @param distance 
		 */
		public ProjectedVertex(int vertex, boolean isA, float distance) {
			this.vertex = vertex;
			this.isA = isA;
			this.distance = distance;
		}
	}
	
	/**  */
	private ProjectedVertex current;	
	
	/**  */
	private Vector2f sweepDir;
	
	/** 
	 * 
	 * @param sweepDir 
	 */
	public EdgeSweep(Vector2f sweepDir) {
		this.sweepDir = new Vector2f(sweepDir);
	}
	
	/**
	 * 
	 * @param vertex 
	 * @param isA 
	 * @param distance 
	 */
	private void insertBackwards(int vertex, boolean isA, float distance) {
		ProjectedVertex svl = new ProjectedVertex(vertex, isA, distance);
		
		if ( current == null ) {
			current = svl;
			return;
		}
		
		while ( current.distance > svl.distance ) {
			if ( current.previous == null ) {
				current.previous = svl;
				svl.next = current;
				current = svl;
				return;
			}
			
			current = current.previous;
		}
		
		svl.next = current.next;
		svl.previous = current;
		current.next = svl;
		
		if ( svl.next != null )
			svl.next.previous = svl;
			
		current = svl;
	}
	
	/**

	 * @param vertex 
	 * @param isA 
	 * @param distance 
	 */
	public void insert(int vertex, boolean isA, float distance) {
		if ( current == null || current.distance <= distance )
			insertForwards(vertex, isA, distance);
		else
			insertBackwards(vertex, isA, distance);
	}
	
	/**
	 * 
	 * @param vertex 
	 * @param isA 
	 * @param distance 
	 */
	private void insertForwards(int vertex, boolean isA, float distance) {
		ProjectedVertex svl = new ProjectedVertex(vertex, isA, distance);
		
		if ( current == null ) {
			current = svl;
			return;
		}
		
		while ( current.distance <= svl.distance ) {
			if ( current.next == null ) {
				current.next = svl;
				svl.previous = current;
				current = svl;
				return;
			}
			
			current = current.next;
		}
		
		svl.next = current;
		svl.previous = current.previous;
		current.previous = svl;
		
		if ( svl.previous != null )
			svl.previous.next = svl;
			
		current = svl;
	}
	
	/** 
	 * 
	 */
	private void goToStart() {
		while ( current.previous != null )
			current = current.previous;
	}

	/**
	 * 
	 */
	public int[][] getOverlappingEdges() {
		if ( current == null )
			return new int[0][2];
		
		goToStart();
		
		CurrentEdges edgesA = new CurrentEdges();
		CurrentEdges edgesB = new CurrentEdges();
		EdgePairs collidingEdges = new EdgePairs();
		
		float lastDist = -Float.MAX_VALUE;
		
		while ( current != null ) {
			if ( current.distance > lastDist ) {
				lastDist = current.distance;
				edgesA.removeScheduled();
				edgesB.removeScheduled();
			}
				
			if ( current.isA ) {
				if ( !edgesA.contains(current.vertex) ) {
					edgesA.addEdge(current.vertex);
					
					int[] edgeListB = edgesB.getEdges(); 
					for ( int i = 0; i < edgeListB.length; i++ )
						collidingEdges.add(current.vertex, edgeListB[i]);
					
				} else {
					edgesA.scheduleRemoval(current.vertex);
				}
			} else {
				if ( !edgesB.contains(current.vertex) ) {
					edgesB.addEdge(current.vertex);
					
					int[] edgeListA = edgesA.getEdges(); 
					for ( int i = 0; i < edgeListA.length; i++ )
						collidingEdges.add(edgeListA[i], current.vertex);
					
				} else {
					edgesB.scheduleRemoval(current.vertex);
				}
			}	
			
			current = current.next;
		}
		
		return collidingEdges.toList();
	}
	
	/** 
	 * 
	 */
	private class CurrentEdges {
		/**  */
		private LinkedEdgeList currentEdges;
		/**  */
		private LinkedEdgeList scheduledForRemoval;
		
		/**
		 * 
		 * @param e 
		 */
		public void addEdge(int e) {
			currentEdges = new LinkedEdgeList(e,currentEdges);
		}
		
		/**
		 * 
		 * @param e 
		 */
		public void scheduleRemoval(int e) {
			if ( currentEdges == null )
				return; 
			
			if ( currentEdges.edge == e ) {
				currentEdges = currentEdges.next;
			} else {
				LinkedEdgeList current = currentEdges.next;
				LinkedEdgeList last = currentEdges;
				
				while ( current != null ) {
					if ( current.edge == e ) {
						last.next = current.next;
						scheduledForRemoval = new LinkedEdgeList(e,scheduledForRemoval);
						return;
					}
					last = current;
					current = current.next;
				}
			}
		}
		
		/** 
		 */
		public void removeScheduled() {
			scheduledForRemoval = null;
		}
		
		/**
		 * 
		 * @param e 
		 * @return 
		 */
		public boolean contains(int e) {
			LinkedEdgeList current = currentEdges;
			while ( current != null ) {
				if ( current.edge == e )
					return true;
				current = current.next;
			}
			
			current = scheduledForRemoval;
			while ( current != null ) {
				if ( current.edge == e )
					return true;
				current = current.next;
			}
			
			return false;
		}
		
		/**
		 * 
		 * @return 
		 */
		public int getNoEdges() {
			int count = 0;
			LinkedEdgeList current = currentEdges;
			while ( current != null ) {
				count++;
				current = current.next;
			}
			
			current = scheduledForRemoval;
			while ( current != null ) {
				count++;
				current = current.next;
			}
			
			return count;
		}
		
		/**
		 * 
		 * @return 
		 */
		public int[] getEdges() {
			int[] returnEdges = new int[getNoEdges()];
			
			int i = 0;
			LinkedEdgeList current = currentEdges;
			while ( current != null ) {
				returnEdges[i] = current.edge;
				i++;
				current = current.next;
			}
			current = scheduledForRemoval;
			while ( current != null ) {
				returnEdges[i] = current.edge;
				i++;
				current = current.next;
			}
			
			return returnEdges;
		}
		
		/**  */
		class LinkedEdgeList {
			/**  */
			public int edge;
			/**  */
			public LinkedEdgeList next;
			
			/**
			 * 
			 * @param edge 
			 * @param next 
			 */
			public LinkedEdgeList(int edge, LinkedEdgeList next) {
				this.edge = edge;
				this.next = next;
			}
		}
	}
	
	/**  */
	private class EdgePairs {
		/** */ 
		private EdgePair first;
		/**  */
		private int size = 0;
		
		/** 
		 * 
		 *
		 * @param idA 
		 * @param idB 
		 */
		public void add(int idA, int idB) {
			first = new EdgePair(idA, idB, first);
			size++;
		}
		
		/**
		 * 
		 * @return 
		 */
		public int[][] toList() {
			int[][] list = new int[size][2];
			
			EdgePair current = first;
			for ( int i = 0; i < size; i++ ) {
				list[i][0] = current.a;
				list[i][1] = current.b;
				
				current = current.next;
			}
			
			return list;
		}
		
		/**  */
		class EdgePair {
			/**  */
			public int a;
			/**  */
			public int b;
			/**  */
			public EdgePair next;
			
			/** 
			 * 
			 * 
			 * @param a 
			 * @param b 
			 * @param next 
			 */
			public EdgePair(int a, int b, EdgePair next) {
				this.a = a;
				this.b = b;
				this.next = next;
			}
		}
	}
	
	/**
	 * 
	 * @param isA 
	 * @param verts 
	 */
	public void addVerticesToSweep(boolean isA, Vector2f[] verts) {
		for ( int i = 0, j = verts.length-1; i < verts.length; j = i, i++ ) {
			float dist = sweepDir.dot(verts[i]);
			
			insert(i, isA, dist);
			insert(j, isA, dist);
		}
	}
	
	/**
	 * 
	 * @return 
	 */
	public Vector2f getSweepDir() {
		return sweepDir;
	}
}
